
#include <stdint.h>
#include <ZUtils.h>
#include <RTPBase.h>
#include <RsFec.h>
#include <ZMemPool.h>
#include <ZLog.h>

int parseFecPacket(RTPPacketT *fecPkt, FecInfoT *fecInfo)
{
    uint8_t *fecData = NULL;
    
    fecData = fecPkt->payload;

    fecInfo->rtpBeginSeq = GetBE16(&fecData[0]);
    fecInfo->rtpEndSeq = GetBE16(&fecData[2]);
    fecInfo->redundNum = GetU8(&fecData[4]);
    fecInfo->redundIdx = GetU8(&fecData[5]);
    fecInfo->fecLen = GetBE16(&fecData[6]);
    fecInfo->rtpLen = GetBE16(&fecData[8]);
    
    return 0;
}

RTPPacketT *createDummyPacket(MemPoolT *pool, uint32_t seqNumU32, uint32_t len)
{    
    RTPPacketT *packet = NULL;
    packet = Palloc(pool, sizeof(*packet));

    packet->data = Palloc(pool, RTP_PACKET_SIZE);
    packet->pool = pool;
    packet->dataLen = len;
    packet->seqNumU32 = seqNumU32;

    return packet;
}

int doFECProcess(RTPGroupT *group)
{
    int ret;
//    int i;
    RsFecT *rsfec = RsFec_Create();
    RsFec_Init(rsfec, group->dataNum, group->fecNum, group->fecInfo.rtpLen);
    ret = RsFec_Decode(rsfec, group->pData, group->pFec, group->lostMap);
/*
    RTPPacketT *posPacket, *nextPacket;
    ZListNodeT *posNode = group->dataList.head;
    for (i = 0; i < group->dataNum; i++)
    {
        posPacket = ZListEntry(posNode, RTPPacketT, groupNode);
        
        if (group->lostMap[i] == 0)
        {
            nextPacket = ZListEntry(posNode, RTPPacketT, groupNode);
            ZListAddBefore(posPacket->node, struct ZListNodeS * pos);
        }
        
        posNode = posNode->next;
    }
  */  
    group->correct = 1;
    return ret;
}

int RTPGroupIsCorrect(RTPGroupT *group)
{
    return group->correct;
}

RTPGroupT *RTPGroupCreate(MemPoolT *pool, RTPPacketT *fecPkt)
{
    RTPGroupT *group = NULL;
    FecInfoT fecInfo;
    int ret;
    
    group = Palloc(pool, sizeof(*group));
    memset(group, 0x00, sizeof(*group));

    if (fecPkt->payloadType != RTP_TYPE_DYNAMIC)
    {
        LogE("Not FEC packet, type(%hhu) can not do this!", fecPkt->payloadType);
        goto failure;
    }

    ret = parseFecPacket(fecPkt, &group->fecInfo);
    if (ret != 0)
    {
        LogE("parser fec info failure!");
        goto failure;
    }

    group->pool = pool;
    group->seqNumU32Min = group->fecInfo.rtpBeginSeq;
    group->seqNumU32Max = group->fecInfo.rtpEndSeq;

    group->dataNum = (group->fecInfo.rtpEndSeq - group->fecInfo.rtpBeginSeq) + 1;
    group->fecNum = group->fecInfo.redundNum;
    group->fecBase = fecPkt->seqNumU32 - group->fecInfo.redundIdx;
    
    ZListInit(&group->dataList);
    ZListInit(&group->fecList);

    group->lostMap = Palloc(pool, (group->dataNum + group->fecNum) * 4);
    group->lostCount = 0;
    
    group->pFec = Palloc(pool, group->fecNum * sizeof(char *));
    group->pData = Palloc(pool, group->dataNum * sizeof(char *));
    
    group->fecExpectSeq = fecPkt->seqNumU32 - fecInfo.redundIdx;
    group->dataExpectSeq = group->seqNumU32Min;
    
    return group;
    
failure:
    if (group)
    {
        Pfree(pool, group);
    }
    return NULL;
}

void RTPGroupDestroy(RTPGroupT *group)
{
    
    Pfree(group->pool, group);
    return ;
}

int RTPGroupAddData(RTPGroupT *group, RTPPacketT *packet)
{
    if (packet->seqNumU32 < group->dataExpectSeq || packet->seqNumU32 > group->seqNumU32Max)
    {
        LogE("Ignore packet No.%u(%u).", packet->seqNumU32, packet->seqNumU16);
        return -1;
    }
    
    if (packet->seqNumU32 == group->dataExpectSeq)
    {
        group->pData[group->dataIndex] = packet->data;
        group->lostMap[group->dataIndex] = 1;
        ZListAddTail(&packet->groupNode, &group->dataList);
        group->dataIndex++;
        group->dataExpectSeq++;
    }
    else /* (dataExpectSeq, seqNumU32Max] */
    {
        int dummyNum = packet->seqNumU32 - group->dataExpectSeq;
        int i;
        for (i = 0; i < dummyNum; i++)
        {   
            RTPPacketT *dummyPacket = NULL;
            dummyPacket = createDummyPacket(group->pool, group->dataExpectSeq, group->fecInfo.rtpLen);
            LogI("seq(%u) drop, add dummy packet(%p).", group->dataExpectSeq, dummyPacket);

            group->pData[group->dataIndex] = dummyPacket->data;
            group->lostMap[group->dataIndex] = 0;
            ZListAddTail(&dummyPacket->groupNode, &group->dataList);
            group->dataIndex++;
            group->dataExpectSeq++;
            group->lostCount++;
        }
        
        group->pData[group->dataIndex] = packet->data;
        group->lostMap[group->dataIndex] = 1;
        ZListAddTail(&packet->groupNode, &group->dataList);
        group->dataIndex++;
        group->dataExpectSeq++;
    }

    if ((group->dataExpectSeq == (group->seqNumU32Max + 1)) /* data packet enought */
        && group->lostCount /* lost packet happen */
        && (group->fecExpectSeq == (group->fecBase + group->fecNum))) /* fec packet enought */
    {
        doFECProcess(group);
    }
    return 0;
}

int RTPGroupAddFec(RTPGroupT *group, RTPPacketT *packet)
{
    if (packet->seqNumU32 < group->fecExpectSeq 
        || packet->seqNumU32 >= group->fecBase + group->fecNum)
    {
        LogE("Ignore packet No.%u(%u)", packet->seqNumU32, packet->seqNumU16);
        return -1;
    }
    
    if (packet->seqNumU32 == group->fecExpectSeq)
    {
        group->pFec[group->fecIndex] = packet->data + 12; /* fec header size 12 bytes */
        group->lostMap[group->dataNum + group->fecIndex] = 1;
        ZListAddTail(&packet->groupNode, &group->fecList);
        group->fecIndex++;
        group->fecExpectSeq++;
    }
    else /* (dataExpectSeq, seqNumU32Max] */
    {
        int dummyNum = packet->seqNumU32 - group->fecExpectSeq;
        int i;
        for (i = 0; i < dummyNum; i++)
        {   
            RTPPacketT *dummyPacket = NULL;
            dummyPacket = createDummyPacket(group->pool, group->fecExpectSeq, group->fecInfo.fecLen);

            group->pFec[group->fecIndex] = dummyPacket->data + 12;
            group->lostMap[group->dataNum + group->fecIndex] = 0;
            ZListAddTail(&dummyPacket->groupNode, &group->fecList);
            group->fecIndex++;
            group->fecExpectSeq++;
        }
        
        group->pFec[group->fecIndex] = packet->data + 12;
        group->lostMap[group->dataNum + group->fecIndex] = 1;
        ZListAddTail(&packet->groupNode, &group->fecList);
        group->fecIndex++;
        group->fecExpectSeq++;
    }

    if ((group->fecExpectSeq == (group->fecBase + group->fecNum)) /* fec packet enought */
        && group->lostCount /* lost packet happen */
        && (group->dataExpectSeq == (group->seqNumU32Max + 1))) /* fec packet enought */
    {
        doFECProcess(group);
    }
    
    return 0;
}

int RTPGroupNotifyDataEnd(RTPGroupT *group)
{
    int dummyNum = group->seqNumU32Max - group->dataExpectSeq;
    int i;
    for (i = 0; i < dummyNum; i++)
    {   
        RTPPacketT *dummyPacket = NULL;
        dummyPacket = createDummyPacket(group->pool, group->dataExpectSeq, group->fecInfo.rtpLen);
    
        group->pData[group->dataIndex] = dummyPacket->data;
        group->lostMap[group->dataIndex] = 0;
        ZListAddTail(&dummyPacket->groupNode, &group->dataList);
        group->dataIndex++;
        group->dataExpectSeq++;
        group->lostCount++;
    }

    if ((group->dataExpectSeq == (group->seqNumU32Max + 1)) /* data packet enought */
        && group->lostCount /* lost packet happen */
        && (group->fecExpectSeq == (group->fecBase + group->fecNum))) /* fec packet enought */
    {
        doFECProcess(group);
    }
    return 0;
}

int RTPGroupNotifyFecEnd(RTPGroupT *group)
{
    int dummyNum = (group->fecBase + group->fecNum + 1) - group->fecExpectSeq;
    int i;
    for (i = 0; i < dummyNum; i++)
    {   
        RTPPacketT *dummyPacket = NULL;
        dummyPacket = createDummyPacket(group->pool, group->fecExpectSeq, group->fecInfo.fecLen);
    
        group->pFec[group->fecIndex] = dummyPacket->data + 12;
        group->lostMap[group->dataNum + group->fecIndex] = 0;
        ZListAddTail(&dummyPacket->groupNode, &group->fecList);
        group->fecIndex++;
        group->fecExpectSeq++;
    }

    if ((group->fecExpectSeq == (group->fecBase + group->fecNum)) /* fec packet enought */
        && group->lostCount /* lost packet happen */
        && (group->dataExpectSeq == (group->seqNumU32Max + 1))) /* fec packet enought */
    {
        doFECProcess(group);
    }
    return 0;
}

int RTPGroupDetectData(RTPGroupT *group, RTPPacketT *packet)
{
    return (packet->seqNumU32 >= group->seqNumU32Min
            && packet->seqNumU32 <= group->seqNumU32Max);
}

int RTPGroupDetectFec(RTPGroupT *group, RTPPacketT *packet)
{
    Z_ASSERT(group);
    Z_ASSERT(packet);
    return (packet->seqNumU16 >= group->fecInfo.rtpBeginSeq
            && packet->seqNumU16 <= group->fecInfo.rtpEndSeq);
}

static int32_t parseRTPPacket(RTPPacketT *packet)
{
    const uint8_t *data = packet->data;
    uint32_t bufSize = packet->dataLen;
    int32_t numCSRCs;
    size_t payloadOffset;
    
    Z_LOG_ASSERT(bufSize >= 12, "bufSize(%u)", bufSize);

    if ((data[0] >> 6) != 2) /*version 2, RFC 1889*/
    {
        LogE(" Unsupported version");
        return -1;
    }

    if (data[0] & 0x20)
    {
        // Padding present.
        size_t paddingLength = data[bufSize - 1];

        if (paddingLength + 12 > bufSize)
        {
            // If we removed this much padding we'd end up with something
            LogE("that's too short to be a valid RTP header.");
            return -1;
        }
        bufSize -= paddingLength;
    }

    numCSRCs = data[0] & 0x0f;
    payloadOffset = 12 + 4 * numCSRCs;

    if (bufSize < payloadOffset)
    {
        LogE("Not enough data to fit the basic header and all the CSRC entries.");
        return -1;
    }

    if (data[0] & 0x10)
    {
        const uint8_t *extensionData;
        size_t extensionLength;
        // Header eXtension present.
        if (bufSize < payloadOffset + 4)
        {
            LogE("Not enough data to fit the basic header, all CSRC entries"
                     "and the first 4 bytes of the extension header.");
            return -1;
        }

        extensionData = &data[payloadOffset];
        extensionLength = 4 * (extensionData[2] << 8 | extensionData[3]);

        if (bufSize < payloadOffset + 4 + extensionLength)
        {
            LogE("format error");
            return -1;
        }

        payloadOffset += 4 + extensionLength;
    }

    packet->payloadType = data[1] & 0x7f;
    
    packet->seqNumU16 = GetBE16(&data[2]);

    packet->ssrc = GetBE32(&data[8]);

    packet->timeStamp = GetBE32(&data[4]);
    packet->payload = packet->data + payloadOffset;
    packet->payloadLen = packet->dataLen - payloadOffset;
    return 0;
}

#define ABS_DIFF(seq1, seq2) \
    (seq1 > seq2 ? seq1 - seq2 : seq2 - seq1)

static uint32_t seqNumConvert(uint32_t seqNumHighest, uint32_t seqNumU16)
{
    uint32_t seq1, seq2, seq3, diff1, diff2, diff3;
    uint32_t ret;

    Z_LOG_ASSERT(seqNumU16 <= 0xffff, "can't not...");
    // Only the lower 16-bit of the sequence numbers are transmitted,
    // derive the high-order bits by choosing the candidate closest
    // to the highest sequence number (extended to 32 bits) received so far.

    if (seqNumHighest == 0)
    {
        return seqNumU16;
    }
    
    seq1 = seqNumU16 | (seqNumHighest & 0xffff0000);
    seq2 = seqNumU16 | ((seqNumHighest & 0xffff0000) + 0x10000);
    seq3 = seqNumU16 | ((seqNumHighest & 0xffff0000) - 0x10000);
    diff1 = ABS_DIFF(seq1, seqNumHighest);
    diff2 = ABS_DIFF(seq2, seqNumHighest);
    diff3 = ABS_DIFF(seq3, seqNumHighest);

    if (diff1 < diff2)
    {
        if (diff1 < diff3)
        {
            // diff1 < diff2 ^ diff1 < diff3
            ret = seq1;
        }
        else
        {
            // diff3 <= diff1 < diff2
            ret = seq3;
        }
    }
    else if (diff2 < diff3)
    {
        // diff2 <= diff1 ^ diff2 < diff3
        ret = seq2;
    }
    else
    {
        // diff3 <= diff2 <= diff1
        ret = seq3;
    }

    return ret;
}

RTPPacketT *RTPPacketCreate(MemPoolT *pool, uint8_t *data, int32_t dataLen,
                                    uint32_t seqU32Base)
{
    RTPPacketT *packet = NULL;
    int ret;
    
    packet = Palloc(pool, sizeof(*packet));
    memset(packet, 0x00, sizeof(*packet));
    packet->pool = pool;
    packet->data = data;
    packet->dataLen = dataLen;
    ret = parseRTPPacket(packet);
    if (ret != 0)
    {
        LogE("Parse failure...");
        goto failure;
    }

    if (seqU32Base == 0)
    {
        seqU32Base = (packet->seqNumU16 != 0) ? (packet->seqNumU16 - 1)
                                              : 0xffff;
    }
    packet->seqNumU32 = seqNumConvert(seqU32Base, packet->seqNumU16);    

    ZAtomicSet(&packet->ref, 1);
    
    return packet;

failure:
    return NULL;
}

RTPPacketT *RTPPacketRefInc(RTPPacketT *packet)
{
    ZAtomicInc(&packet->ref);
    return packet;
}

void RTPPacketRefDec(RTPPacketT *packet)
{
    if (ZAtomicDec(&packet->ref) == 0)
    {
        Pfree(packet->pool, packet->data);
        Pfree(packet->pool, packet);
    }
    return ;
}

